// Have include guard.
#ifndef CANJAGUARWRAPPER_HPP
#define CANJAGUARWRAPPER_HPP

#include <boost/smart_ptr/shared_ptr.hpp>
#include "WPILib.h"
#include "PidMotorController.hpp"

/**
 *  The CanJaguarWrapper class wraps a CANJaguar into a PidMotorController so FPK can use CANJaguars.
 */
class CanJaguarWrapper :
	public PidMotorController
{
	public:
		/**
		 *  The EncoderKind enum is used to specify what kind of position or distance sensor the CAN Jaguar is using. 
		 */
		enum EncoderKind
		{
			/// The Nothing encoder kind specifies that no encoder is used.
			Nothing,
			
			/// The Encoder encoder kind specifies that a normal encoder is used.
			Encoder,
			
			/// The InvertedEncoder encoder kind specifies that an inverted encoder is used.
			InvertedEncoder,
			
			/// The QuadEncoder encoder kind specifies that a quadrature encoder is used.
			QuadEncoder,
			
			/// The Potentiometer encoder kind specifies that a potentiometer is used.
			Potentiometer
		};
		
		CanJaguarWrapper(CANJaguar *canJaguar, EncoderKind encoderKind, bool canDeleteCanJaguar = false);
		CanJaguarWrapper(CANJaguar &canJaguar, EncoderKind encoderKind);
		CanJaguarWrapper(boost::shared_ptr<CANJaguar> &canJaguar, EncoderKind encoderKind);
		CanJaguarWrapper(CANJaguar *canJaguar, bool canDeleteCanJaguar = false);
		CanJaguarWrapper(CANJaguar &canJaguar);
		CanJaguarWrapper(boost::shared_ptr<CANJaguar> &canJaguar);
		
		/* The three following function declarations are derived from 
		 * WPILib, which is licenced under the licence specified
		 * in "Licence-WPILib.txt".
		 */
		virtual void Set(float speed, UINT8 syncGroup = 0);
		virtual float Get();
		virtual void Disable();
		
		virtual double GetSpeed();
		virtual double GetPosition();
		virtual void Enable(double initialEncoderPosition = 0);
		virtual ControlMode GetControlMode();
		virtual void SetControlMode(ControlMode mode);
		virtual void SetEncodersCodesPerRevolution(UINT16 codesPerRevolution);
		virtual void SetPid(double p, double i, double d);
		virtual double GetP();
		virtual double GetI();
		virtual double GetD();
	private:
		boost::shared_ptr<CANJaguar> m_canJaguar;
		
		void SetSpeedReference(CANJaguar::SpeedReference speedReference);
		void SetPositionReference(CANJaguar::PositionReference positionReference);
		void SetupReference(EncoderKind encoderKind);
};

#endif // #ifndef CANJAGUARWRAPPER_HPP
